/*
This file is part of the Intrael Project which provides computer vision for the web.
Copyright (C) 2012 Yannis Gravezas. You can contact the author at wizgrav@gmail.com.

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Affero General Public License as
published by the Free Software Foundation, either version 3 of the
License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Affero General Public License for more details.

You should have received a copy of the GNU Affero General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <math.h>
#define VMAJOR 2
#define VMINOR 0

int die = 0;
#define NULL    0
#define EOF     (-1)
typedef WSABUF IOVEC;
#define WSAEAGAIN WSAEWOULDBLOCK
#define NONBLOCKING(s) ioctlsocket(s, FIONBIO, &NonBlock)
#define INITSOCKET(s)   if ((s = WSASocket(AF_INET, SOCK_STREAM, 0, NULL, 0, WSA_FLAG_OVERLAPPED)) == INVALID_SOCKET) exit(1)
#define SIGEXIT()       die=1;\
	WSACleanup(); \
	return(TRUE)
#define VSEND(hbuf,bufy,hlen,leny)\
	sdata[0].buf = (hbuf)+client->b; \
	sdata[0].len = (hlen)-client->b; \
	sdata[1].buf = bufy; \
	sdata[1].len = (leny); \
if (WSASend(client->s, sdata, 2, &stmp, NULL, NULL, NULL)) stmp = -1
#define VSND(hbuf,hlen,leny)\
	sdata[0].buf = (hbuf)+client->b - (hlen); \
	sdata[0].len = (leny)-client->b + (hlen); \
if (WSASend(client->s, sdata, 1, &stmp, NULL, NULL, NULL)) stmp = -1
typedef int in_addr_t;
#define ERR(s, c) if(opterr){\
		fputs(s, stderr);\
		fputc(c,stderr);\
		fputc('\n',stderr);\
	}

#include "queue.h"
#include "jpeglib.h"

#define MISSED 3
#define MAX_BYTES 32000
#define FRAME_PIXELS 76800
#define MIN(a, b)  (((a) < (b)) ? (a) : (b))
#define MAX(a, b)  (((a) > (b)) ? (a) : (b))
#define CLAMP(x, low, high)  (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x)))
#define IOPT(arg,var,min,max)\
	itmp = _wtoi(arg); \
	var = CLAMP(itmp,min,max);\
	break
#define COPT(ch,min,max,optarg)\
	itmp = _wtoi(optarg); \
	cbuf[(int)ch] = CLAMP(itmp,min,max);\
	break
#define PCK()\
	frame[0] =  (raw[0]<<3)  | (raw[1]>>5);\
	frame[1] = ((raw[2]<<9)  | (raw[3]<<1) | (raw[4]>>7) ) & 2047;\
	frame[2] = ((raw[5]<<7)  | (raw[6]>>1) )           & 2047;\
	frame[3] = ((raw[8]<<5)  | (raw[9]>>3) )           & 2047;\
	frame[4] =  (raw[11]<<3)  | (raw[12]>>5);\
	frame[5] = ((raw[13]<<9)  | (raw[14]<<1) | (raw[15]>>7) ) & 2047;\
	frame[6] = ((raw[16]<<7)  | (raw[17]>>1) )           & 2047;\
	frame[7] = ((raw[19]<<5)  | (raw[20]>>3) )           & 2047
#define IF_THR(idx)\
	da=(frame[idx]).depth;\
	if(da > rawz && da < rawZ)
#define IF_EXT(idx)\
	da=frame[idx].depth;\
	if(da > drz[idx] && da < drZ[idx])
#define EPROC(id)\
	X=i+id;\
	run_y[n] = y;\
	run_z[n] = dz;\
	run_Z[n] = dZ;\
	run_zv[n] = dzv;\
	run_Zv[n] = dZv;\
	run_sum[n]=running;\
	run_e[n] = X;\
	run_ev[n] = depth[X-1].depth;\
	if(!rt)	rt=n;\
	if(rs){\
		for(running = rs;running != re;running++){\
			if(run_e[running] < x) rs=running;\
			else if(run_s[running] < X){\
				da=run_label[running];\
				if(!label){\
					label = da;\
					sr=running;\
				}else if(da < label) label=da;\
				er=running;\
			}else break;\
		}\
		if(label){\
			label=r_label[label];\
			run_label[n] = label;\
			do{\
				r_label[run_label[sr]]  = label;\
			}while(sr++ != er);\
			label=0;\
		}else{\
			run_label[n] = l;\
			r_label[l] = l;\
			l++;\
		}\
	}else{\
		run_label[n] = l;\
		r_label[l] = l;\
		l++;\
	}\
	running=0;\
	n++
#define PROC(id)\
	gray[i+id] = 255 - ((da & 8191) >> 5);\
	if(running){\
		running += da;\
		if(da < dzv){\
			dz = i+id;\
			dzv = (uint16_t) da;\
		}else if(da > dZv){\
			dZ= i+id;\
			dZv= (uint16_t) da;\
		}\
	}else{\
		x=i+id;\
		dZv = (uint16_t) da;\
		dzv = (uint16_t) da;\
		running = (uint16_t) da;\
		dz=x;\
		dZ=x;\
		run_s[n] = x;\
		run_sv[n] = (uint16_t) da;\
	}\
	}else if(running){\
		EPROC(id)
#define IF_NEXT()\
	i+=8;\
	if(i < BOUNDS.X)
#define JUMP()\
	if(running){ EPROC(0); }\
	rs = rt;\
	rt=0;\
	re = n;\
	if(++y >= Y) break;\
	i=BOUNDS.x;\
	frame+=8+320-BOUNDS.X+i;\
	depth=frame
#define CFRAME(frame,head,s)\
	frame = (frame_t *)malloc(sizeof(frame_t));\
	frame->h = head;\
	head->c++;\
	frame->c=0;\
	frame->d=0;\
	frame->t=s
#define FSWAP(frame,nframe)\
	frame = (frame_t *) InterlockedExchangePointer((PVOID *) &nframe, frame)
#define FSEL(frame,nframe,cls,entr)\
	if (tframe){ free(tframe);}\
	tframe = (frame_t *)InterlockedExchangePointer((PVOID *)&nframe, NULL);\
	if (tframe){\
		if (frame && frame->c == 0) free(frame); \
		frame = tframe; \
	}\
    if (frame) { SEL(cls, entr, frame); }\
	tframe=NULL
#define FCHK(cls,entr)\
	LIST_INSERT_HEAD(&cls, client, entr);\
	LIST_REMOVE(client,entries);\
	FD_CLR(client->s,&master)							 
#define FRAME40(frame)\
	client->f=frame;\
	frame->c++;\
	client->c=0;\
	FD_SET(client->s,&streaming);\
	fdmax=MAX(client->s,fdmax)
#define SEL(lst,entry,frame)\
	if(!LIST_EMPTY(&lst)){\
		if(frame){\
			LIST_FOREACH(client, &lst, entry){\
				if(client->t == frame->t) continue;\
				client->f=frame;\
				frame->c++;\
				client->c=0;\
				client->t=frame->t;\
				FD_SET(client->s,&streaming);\
				FD_SET(client->s,&master);\
				fdmax=MAX(client->s,fdmax);\
				LIST_REMOVE(client,entry);\
				LIST_INSERT_HEAD(&clients, client, entries);\
			}\
		}\
	}
#define DFRAME()\
do{\
client->f->d++; \
if (client->f->d == client->f->c){\
	switch (client->f->h->t){\
		case 1: if (client->f == gframe) gframe = NULL; break; \
		case 2: if (client->f == vframe) vframe = NULL; break; \
		default: if (client->f == dframe) dframe = NULL; break; \
	}\
	free(client->f); \
}\
client->f = NULL;\
} while (0)
#define STREAM(cls,entry,mod,off)\
	if(client->m){\
		if(client->b < client->f->h->ml){\
			VSEND(client->f->h->mbuf,client->f->buf + mod,client->f->h->ml,client->f->l - mod);\
		}else{\
			VSND(client->f->buf+mod,client->f->h->ml,client->f->l-mod);\
		}\
	}else{\
		if(client->b < client->f->h->sl){\
			VSEND(client->f->h->sbuf,client->f->buf,client->f->h->sl,client->f->l);\
		}else{\
			VSND(client->f->buf,client->f->h->sl,client->f->l);\
		}\
	}\
	if(stmp > 0){\
		client->b += stmp;\
		if(client->b == (client->m ? (client->f->l + client->f->h->ml - mod) : (client->f->l + client->f->h->sl))){\
				stmp = (off);\
				DFRAME();\
				FD_CLR(client->s,&streaming);\
				if(client->m){\
					client->b = stmp;\
					FD_CLR(client->s,&master);\
					LIST_INSERT_HEAD(&cls, client, entry);\
					LIST_REMOVE(client,entries);\
				}else{\
					client->b=0;\
					fdmax=MAX(client->s,fdmax);\
				}\
		}else fdmax=MAX(client->s,fdmax);\
	}else{\
		 stmp=errno;\
		 if(stmp != WSAEWOULDBLOCK || stmp != WSAEAGAIN || stmp != WSAEINTR ) kick=1; else fdmax=MAX(client->s,fdmax);\
	}\
	break
#define HSET(head,strs,strm,type)\
	head = (header_t *)malloc(sizeof(header_t));\
	head->sbuf = (char *)malloc(256);\
	head->sl = sprintf_s(head->sbuf,256,strs,VMAJOR,VMINOR);\
	head->mbuf = (char *)malloc(256);\
	head->ml = sprintf_s(head->mbuf,256,strm,VMAJOR,VMINOR);\
	head->t = type;\
	head->c =0
#define JSET(inf,space,comp)\
	memset(&inf, 0, sizeof(inf)); \
	inf.err = jpeg_std_error(&jerr); \
	jpeg_create_compress(&inf); \
	jpeg_memory_dest(&inf); \
	inf.image_width = 320; \
	inf.image_height = 240; \
	inf.dct_method = JDCT_FASTEST; \
	inf.input_components = comp; \
	inf.in_color_space = space; \
	jpeg_set_defaults(&inf); \
	jpeg_set_quality(&inf, 75, TRUE)
#define FAIL(error) {\
		ERR(error,' ');\
		return 1;\
	}
#define STR_JSON_M "HTTP/1.1 200 OK\r\nConnection: Close\r\nServer: Intrael %d.%d\r\nCache-Control: no-cache,  no-store\r\nAccess-Control-Allow-Origin: *\r\nContent-type: text/event-stream\r\n\r\ndata:"
#define STR_JSON_S "HTTP/1.1 200 OK\r\nServer: Intrael %d.%d\r\nCache-Control: no-cache,  no-store\r\nAccess-Control-Allow-Origin: *\r\n"
#define STR_JSON "Content-Type: application/json\r\nContent-Length: %7d\r\n\r\n"
#define STR_JPEG_M "HTTP/1.1 200 OK\r\nServer: Intrael %d.%d\r\nConnection: Close\r\nCache-Control: no-cache,  no-store\r\nAccess-Control-Allow-Origin: *\r\nContent-type: multipart/x-mixed-replace; boundary=INTRAEL\r\n\r\n--INTRAEL\r\n"
#define STR_JPEG_S "HTTP/1.1 200 OK\r\nServer: Intrael %d.%d\r\nConnection: close\r\nCache-Control: no-cache,  no-store\r\nAccess-Control-Allow-Origin: *\r\n"
#define STR_JPEG "Content-Type: image/jpeg\r\nContent-Length: %7d\r\n\r\n"
#define STR_DUMP "HTTP/1.1 200 OK\r\nServer: Intrael %d.%d\r\nCache-Control: no-cache,  no-store\r\nAccess-Control-Allow-Origin: *\r\nContent-Type: application/octet-stream\r\nContent-Length: 153600\r\n\r\n"
#define STR_BMP "HTTP/1.1 200 OK\r\nServer: Intrael %d.%d\r\nCache-Control: no-cache,  no-store\r\nAccess-Control-Allow-Origin: *\r\nContent-Type: application/octet-stream\r\nContent-Length: 230400\r\n\r\n"
#define STR_REG "HTTP/1.1 200 OK\r\nServer: Intrael %d.%d\r\nCache-Control: no-cache,  no-store\r\nAccess-Control-Allow-Origin: *\r\nContent-Type: application/octet-stream\r\nContent-Length: 317200\r\n\r\n"
#define STR_404 "HTTP/1.1 404 Not Found\r\nServer: Intrael %d.%d\r\nAccess-Control-Allow-Origin: *\r\nContent-Type: text/html\r\nContent-Length: 91\r\n\r\n"
#define STR_404B "<html><head><title>Intrael - Not Found</title></head><body><h1>Not Found</h1></body></html>"
#define STR_403 "HTTP/1.1 403 Forbidden\r\nServer: Intrael %d.%d\r\nAccess-Control-Allow-Origin: *\r\nContent-Type: text/html\r\nContent-Length: 91\r\n\r\n"
#define STR_403B "<html><head><title>Intrael - Forbidden</title></head><body><h1>Forbidden</h1></body></html>"
#define STR_503 "HTTP/1.1 503 Service Unavailable\r\nServer: Intrael %d.%d\r\nAccess-Control-Allow-Origin: *\r\nContent-Type: text/html\r\nContent-Length: 111\r\n\r\n"
#define STR_503B "<html><head><title>Intrael - Service Unavailable</title></head><body><h1>Service Unavailable</h1></body></html>"

typedef struct header_t {
	uint32_t sl, ml, t, c;
	char *sbuf, *mbuf;
} header_t;

typedef struct frame_t {
	uint32_t l,c,t,d;
	header_t *h;
	char buf[65535];
	LIST_ENTRY(frame_t) entries;
	LIST_ENTRY(frame_t) dentries;
} frame_t;

typedef struct client_t {
	in_addr_t a;
	uint32_t b, t, m, c, n;
	SOCKET s;
	frame_t *f;
	LIST_ENTRY(client_t) entries;
	LIST_ENTRY(client_t) dentries;
	LIST_ENTRY(client_t) ventries;
	LIST_ENTRY(client_t) gentries;
} client_t;

typedef struct {
	struct jpeg_destination_mgr pub;
	JOCTET* buffer;
	int bufsize;
	size_t datasize;
	uint16_t* outsize;
	uint16_t* offset;
} memory_destination_mgr;

typedef memory_destination_mgr* mem_dest_ptr;

__declspec(align(16)) uint16_t depth[320];
__declspec(align(16)) uint8_t rgb[320*4];
__declspec(align(16)) uint8_t black[320];
__declspec(align(16)) uint8_t gray[320];

header_t *vhead, *ghead, *dhead;
frame_t *ndframe = NULL;
frame_t *ngframe = NULL;
frame_t *nvframe=NULL;
uint8_t depth_to_gray[2048],quality,run_y[FRAME_PIXELS/2+1];
uint16_t depth_ref[FRAME_PIXELS],depth_to_raw[10000],depth_to_mm[2048],run_zv[FRAME_PIXELS/2+1],run_Zv[FRAME_PIXELS/2+1],run_sv[FRAME_PIXELS/2+1],run_ev[FRAME_PIXELS/2+1],run_s[FRAME_PIXELS/2+1],run_e[FRAME_PIXELS/2+1],run_z[FRAME_PIXELS/2+1],run_Z[FRAME_PIXELS/2+1],run_label[FRAME_PIXELS/2+1],r_label[FRAME_PIXELS/2+1],l_checked[FRAME_PIXELS/2+1],l_runs[FRAME_PIXELS/2+1],l_pos_x[FRAME_PIXELS/2+1],l_pos_X[FRAME_PIXELS/2+1],l_pos_y[FRAME_PIXELS/2+1],l_pos_Y[FRAME_PIXELS/2+1],l_pos_z[FRAME_PIXELS/2+1],l_pos_Z[FRAME_PIXELS/2+1];
uint32_t run_sum[FRAME_PIXELS/2+1],l_cx[FRAME_PIXELS/2+1],l_cy[FRAME_PIXELS/2+1],l_sum[FRAME_PIXELS/2+1],l_count[FRAME_PIXELS/2+1],l_vrun[FRAME_PIXELS/2+1];
int32_t active,Cnt;


